﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SmartMathLibrary.MinPack
{
    /// <summary>
    /// Solves a system of equation in the least squares sense.
    /// </summary>
    [Serializable]
    public static class QrsolvClass
    {
        /// <summary>
        /// Solves a system of equation in the least squares sense.
        /// </summary>
        /// <param name="n">Input variable set to the order of r.</param>
        /// <param name="r">An n by n array. On input the full upper triangle must contain the full upper triangle of the matrix r. On output the full upper triangle is unaltered, and the strict lower triangle contains the strict upper triangle (transposed) of the upper triangular matrix s.</param>
        /// <param name="ipvt">An integer input array of length n which defines the permutation matrix p such that a*p = q*r. Column j of p is column ipvt(j) of the identity matrix.</param>
        /// <param name="diag">An input array of length n which must contain the diagonal elements of the matrix d.</param>
        /// <param name="qtb">An input array of length n which must contain the first n elements of the vector (q transpose)*b.</param>
        /// <param name="x">An output array of length n which contains the least squares solution of the system a*x = b, d*x = 0.</param>
        /// <param name="sdiag">An output array of length n which contains the diagonal elements of the upper triangular matrix s.</param>
        /// <param name="wa">A work array of length n.</param>
        public static void Qrsolv(int n, double[,] r, int[] ipvt, double[] diag,
                                  double[] qtb, double[] x, double[] sdiag, double[] wa)
        {
            int i, j, jp1, k, kp1, l;
            double qtbpj, sum, temp, dsin, dcos, dtan, dcotan;

            for (j = 0; j < n; j++)
            {
                for (i = j; i < n; i++)
                {
                    r[j, i] = r[i, j];
                }
                x[j] = r[j, j];
                wa[j] = qtb[j];
            }
            for (j = 0; j < n; j++)
            {
                l = ipvt[j];
                if (diag[l] != 0.0)
                {
                    for (k = j; k < n; k++)
                    {
                        sdiag[k] = 0.0;
                    }
                    sdiag[j] = diag[l];
                    qtbpj = 0.0;
                    for (k = j; k < n; k++)
                    {
                        if (sdiag[k] == 0.0)
                        {
                            continue;
                        }
                        if (Math.Abs(r[k, k]) < Math.Abs(sdiag[k]))
                        {
                            dcotan = r[k, k] / sdiag[k];
                            dsin = 1.0 / Math.Sqrt(1.0 + dcotan * dcotan);
                            dcos = dsin * dcotan;
                        }
                        else
                        {
                            dtan = sdiag[k] / r[k, k];
                            dcos = 1.0 / Math.Sqrt(1.0 + dtan * dtan);
                            dsin = dcos * dtan;
                        }
                        r[k, k] = dcos * r[k, k] + dsin * sdiag[k];
                        temp = dcos * wa[k] + dsin * qtbpj;
                        qtbpj = -dsin * wa[k] + dcos * qtbpj;
                        wa[k] = temp;
                        kp1 = k + 1;
                        if (n <= kp1)
                        {
                            continue;
                        }
                        for (i = kp1; i < n; i++)
                        {
                            temp = dcos * r[k, i] + dsin * sdiag[i];
                            sdiag[i] = -dsin * r[k, i] + dcos * sdiag[i];
                            r[k, i] = temp;
                        }
                    }
                }
                sdiag[j] = r[j, j];
                r[j, j] = x[j];
            }
            int nsing = n;
            for (j = 0; j < n; j++)
            {
                if ((sdiag[j] == 0.0) && (nsing == n))
                {
                    nsing = j;
                }
                if (nsing < n)
                {
                    wa[j] = 0.0;
                }
            }
            if (nsing >= 1)
            {
                for (k = 0; k < nsing; k++)
                {
                    j = nsing - k - 1;
                    sum = 0.0;
                    jp1 = j + 1;
                    if (nsing > jp1)
                    {
                        for (i = jp1; i < nsing; i++)
                        {
                            sum += r[j, i] * wa[i];
                        }
                    }
                    wa[j] = (wa[j] - sum) / sdiag[j];
                }
            }
            for (j = 0; j < n; j++)
            {
                l = ipvt[j];
                x[l] = wa[j];
            }
        }
    }
}